#!/usr/bin/env python
# -*- coding: utf-8 -*-
import copy
import argparse
from cmath import pi
import math
from turtle import left

import cv2 as cv
import numpy as np
import mediapipe as mp
import serial
import serial.tools.list_ports 

from utils import CvFpsCalc
left_hand=[0,0]
right_hand=[0,0]
left_shurder=[0,0]
right_shurder=[0,0]

left_waist=[0,0]
right_waist=[0,0]
left_knee=[0,0]
right_knee=[0,0]
cam_width=500
cam_height=400


L=120.0  #连杆长度 单位mm
OC=60.0  #两个舵机旋转轴距离 mm
W=422.5  #左右舵机距离 mm
py_left2right=210.0  #皮影臂展长度  mm
leg_max=15 #腿部最大开合角度 度

step_count=1600  #步进电机转一圈 需要1600个脉冲   行程是60mm
step_one=60
step_length=360  #步进电机行程 36cm  360mm
step_allcount=(step_length/step_one)*1600
step_x=0.5*step_allcount  # 步进电机 默认位置为正中心 
step_error=10

#步进电机控制函数
def step_to(x):
  global step_x
  step_t=int((x/cam_width)*step_allcount)
  step_x_error=step_t-step_x
  if abs(step_x_error)>step_error:
    #ser.write(("x"+str(step_x_error)).encode())
    step_x=step_t

#设置左手的两个舵机角度
def set_left_xy(x, y):
  KO=math.sqrt(x**2+y**2)
  #print("KO:",KO)
  alpha1=math.atan(x/y)
  if (alpha1<0):
      alpha1=pi+alpha1
  print("alpha1:",alpha1*180/pi)
  alpha2=math.acos((KO**2)/(2*L*KO))
  print("alpha2:",alpha2*180/pi)
  alpha=min(int((alpha1+alpha2)*180/pi),180)
  print("a:",alpha)

  #beta1=math.acos((2*L**2-KO**2)/(2*L**2))
  #print("beta1:",beta1)
  KC=abs(math.sqrt(OC**2+KO**2-2*KO*OC*math.cos(alpha1)))
  print("KC:",KC)
  #theta1=math.acos((OC**2+KC**2-KO**2)/(2*OC*KC))
  theta1=math.atan(x/(OC-y))
  if (theta1<0):
      theta1=pi+theta1
  print("theta1:",theta1*180/pi)
  theta2=math.acos((KC**2)/(2*KC*L))
  theta=min(int((pi-theta1-theta2)*180/pi),180)
  print("b: ",theta)
  #ser.write(("a"+str(alpha)).encode())
  #ser.write(("b"+str(theta)).encode())

#设置右手的两个舵机角度
def set_right_xy(x, y):
  KO=math.sqrt(x**2+y**2)
  #print("KO:",KO)
  alpha1=math.atan((-x/y))
  if (alpha1<0):
      alpha1=pi+alpha1
  print("alpha1:",alpha1*180/pi)
  
  alpha2=abs(math.acos((KO**2)/(2*L*KO)))
  print("alpha2:",alpha2*180/pi)

  alpha=min(int((pi-alpha1-alpha2)*180/pi),180)
  print("f:",alpha)

  #beta1=math.acos((2*L**2-KO**2)/(2*L**2))
  #print("beta1:",beta1)
  KC=abs(math.sqrt(OC**2+KO**2-2*KO*OC*math.cos(alpha1)))
  #print("KC:",KC)
  #theta1=math.acos((OC**2+KC**2-KO**2)/(2*OC*KC))
  theta1=math.atan(-x/(OC-y))
  if (theta1<0):
      theta1=pi+theta1
  print("theta1:",theta1*180/pi)
  theta2=abs(math.acos((KC**2)/(2*KC*L)))
  print("theta2:",theta2*180/pi)
  theta=min(int((theta1+theta2)*180/pi),180)
  print("e: ",theta)
  #ser.write(("f"+str(alpha)).encode())
  #ser.write(("e"+str(theta)).encode())

#set_left_xy(140,-90)
# set_left_xy(170,-40)
# set_left_xy(170,100)
set_right_xy(-140,-40)
# set_left_xy(170,100)